#ifndef EXAMPLES_RCLCPP_MINIMAL_ACTION_CLIENT_NAVTOPOSECLIENT_H
#define EXAMPLES_RCLCPP_MINIMAL_ACTION_CLIENT_NAVTOPOSECLIENT_H

#include <chrono>
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include <geometry_msgs/msg/pose2_d.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>


using ClientT = nav2_msgs::action::NavigateToPose;
using NavigationAction = nav2_msgs::action::NavigateToPose;

class NavToPose : public rclcpp::Node
{
public:

    explicit NavToPose(const rclcpp::NodeOptions &node_options = rclcpp::NodeOptions())
            : Node("NavToPose_action_client", node_options)
    {
        this->mv_base_client = rclcpp_action::create_client<ClientT>(
                this->get_node_base_interface(),
                this->get_node_graph_interface(),
                this->get_node_logging_interface(),
                this->get_node_waitables_interface(),
                "navigate_to_pose");
    }

    bool is_finish_nav() const
    {
        return this->is_finish_nav_;
    }

    bool is_goal() const
    {
        return this->is_goal_;
    }

    void send_goal(geometry_msgs::msg::Pose2D map_oriented_pose)
    {
        this->is_finish_nav_ = false;
        this->is_goal_ = false;
        ClientT::Goal move_base_goal;
        move_base_goal.pose.header.frame_id = "map";
        move_base_goal.pose.header.stamp = rclcpp::Time();
        move_base_goal.pose.pose.position.x = map_oriented_pose.x;
        move_base_goal.pose.pose.position.y = map_oriented_pose.y;
        move_base_goal.pose.pose.orientation.z = std::sin(map_oriented_pose.theta / 2);
        move_base_goal.pose.pose.orientation.w = std::cos(map_oriented_pose.theta / 2);
        while (!this->mv_base_client->wait_for_action_server(std::chrono::seconds(5)))
        {
            RCLCPP_INFO(this->get_logger(), "等待Action服务上线。");
            return;
        }
        // RCLCPP_INFO(this->get_logger(), "Sending goal");
        rclcpp_action::Client<ClientT>::SendGoalOptions send_goal_options;
        send_goal_options.result_callback =
                std::bind(&NavToPose::result_callback, this, std::placeholders::_1);
        auto goal_handle_future = this->mv_base_client->async_send_goal(move_base_goal, send_goal_options);
    }

private:
    rclcpp_action::Client<ClientT>::SharedPtr mv_base_client;
    bool is_finish_nav_ = false;
    bool is_goal_ = false;

    void result_callback(const rclcpp_action::ClientGoalHandle<NavigationAction>::WrappedResult &result)
    {
        switch (result.code)
        {
            case rclcpp_action::ResultCode::SUCCEEDED:
                this->is_goal_ = true;
                std::cout << std::endl << "处理成功！" << std::endl;
                break;
            case rclcpp_action::ResultCode::ABORTED:
                std::cout << std::endl << "处理失败！" << std::endl;
                break;
            case rclcpp_action::ResultCode::CANCELED:
                std::cout << std::endl << "取消了！" << std::endl;
                break;
            case rclcpp_action::ResultCode::UNKNOWN:
                std::cout << std::endl << "未知的！ UNKNOWN " << std::endl;
                break;
            default:
                std::cout << std::endl << "未知的！" << std::endl;
                break;
        }
        this->is_finish_nav_ = true;
        // std::cout << "GoalResultCallback pass!!!" << std::endl;
    }
};

#endif //EXAMPLES_RCLCPP_MINIMAL_ACTION_CLIENT_NAVTOPOSECLIENT_H

